// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#include <tf2_ros/transform_listener.h>
// #include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/utilities.hpp>
// #include <serial_driver/serial_driver.hpp>
#if defined(ROS_DISTRO_FOXY)
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#elif defined(ROS_DISTRO_HUMBLE)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#error "Unsupported or Unknown ROS Distro"
#endif
// C++ system
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <cmath>
#include "rm_serial_driver/crc.hpp"
#include "rm_serial_driver/packet.hpp"
#include "rm_serial_driver/rm_serial_driver.hpp"
#include "rm_serial_driver/SerialEncoder.hpp"


#include <unistd.h>
#include <sys/ioctl.h>
#include <chrono>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <asm/termbits.h> /* struct termios2 */

#include "auto_aim_interfaces/msg/dangle_shoot.hpp"
#include "std_msgs/msg/u_int16.hpp"
#include "auto_aim_interfaces/msg/cmd_up_yaw_volt_shoot.hpp"


#define CALIBERATING_GUN

namespace rm_serial_driver
{
RMSerialDriver::RMSerialDriver(const rclcpp::NodeOptions & options)
// : Node("rm_serial_driver", options),
: Node("rm_serial_driver", options)
  // owned_ctx_{new IoContext(2)},
  // serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx_)}
{
  RCLCPP_INFO(get_logger(), "Start RMSerialDriver!");

  getParams();
  last_aim_info_time_ = std::chrono::steady_clock::now();

  tf_buffer_ =
    std::make_unique<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ =
    std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);


  // TF broadcaster
  timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  // Create Publisher
  latency_pub_ = this->create_publisher<std_msgs::msg::Float64>("latency", 10);
  marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("aiming_point", 10);

  // Detect parameter client
  detector_param_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this, "armor_detector");

  // Tracker reset service client
  reset_tracker_client_ = this->create_client<std_srvs::srv::Trigger>("tracker/reset");

  ug_ctr_subscription_ = this->create_subscription<auto_aim_interfaces::msg::CmdUpYawVoltShoot>(
    "cmd_up_yaw_volt_shoot", 
    10, 
    std::bind(&RMSerialDriver::ug_ctr_call_back, this, std::placeholders::_1));

  this->declare_parameter<std::string>("direct_dangle_shoot_topic_","/dangle_shoot");
  this->get_parameter("direct_dangle_shoot_topic_",   direct_dangle_shoot_topic_);

  // Create Subscription
  target_sub_ = this->create_subscription<auto_aim_interfaces::msg::DangleShoot>(
    direct_dangle_shoot_topic_, rclcpp::SensorDataQoS(),
    std::bind(&RMSerialDriver::sendData, this, std::placeholders::_1));

  // float_ctr_publisher_ = this->create_publisher<auto_aim_interfaces::msg::DangleShoot>("dangle_shoot", 10);

  // receive_thread_ = std::thread(&RMSerialDriver::receiveData, this);
  try {
    usb_can_tty_fd = adapter_init(device_name_.c_str(), CANUSB_TTY_BAUD_RATE_DEFAULT);
    // serial_driver_->init_port(device_name_, *device_config_);
    // if (!serial_driver_->port()->is_open()) {
    //   serial_driver_->port()->open();
    //   // 新建一个线程来接收串口数据

    //   // receive_thread_ = std::thread(&RMSerialDriver::receiveData, this);
    // }
    
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(
      get_logger(), "Error creating serial port: %s - %s", device_name_.c_str(), ex.what());
    reopenPort();
    
    // throw ex;
  }

  try
  {
    usb_can_tty_fd = adapter_init(device_name_.c_str(), CANUSB_TTY_BAUD_RATE_DEFAULT);
    if(usb_can_tty_fd == -1)
      throw std::runtime_error("usb_can_tty_fd is -1");
  }
  catch(const std::exception& ex)
  {
    RCLCPP_ERROR(
      get_logger(), "Error creating serial port: %s - %s", device_name_.c_str(), ex.what());
    throw ex;
  }

  // init settings
  CANUSB_SPEED speed = CANUSB_SPEED_1000000;
  command_settings(usb_can_tty_fd, speed, CANUSB_MODE_NORMAL, CANUSB_FRAME_STANDARD);

  aiming_point_.header.frame_id = "odom";
  aiming_point_.ns = "aiming_point";
  aiming_point_.type = visualization_msgs::msg::Marker::SPHERE;
  aiming_point_.action = visualization_msgs::msg::Marker::ADD;
  aiming_point_.scale.x = aiming_point_.scale.y = aiming_point_.scale.z = 0.12;
  aiming_point_.color.r = 1.0;
  aiming_point_.color.g = 1.0;
  aiming_point_.color.b = 1.0;
  aiming_point_.color.a = 1.0;
  aiming_point_.lifetime = rclcpp::Duration::from_seconds(0.1);


  // callback
  timer_ = this->create_wall_timer(std::chrono::milliseconds(50),
             std::bind(&RMSerialDriver::timerCallback, this));
}

RMSerialDriver::~RMSerialDriver()
{
  if (receive_thread_.joinable()) {
    receive_thread_.join();
  }
  close(usb_can_tty_fd);
  // if (serial_driver_->port()->is_open()) {
  //   serial_driver_->port()->close();
  // }

  // if (owned_ctx_) {
  //   owned_ctx_->waitForExit();
  // }
}


// void RMSerialDriver::predict_and_gen_ctr(const auto_aim_interfaces::msg::Target::SharedPtr &msg, ctr_data_raw_float & ctr, float pipeline_process_latency_ms) {
//   // pipeline_process_latency is the time from image acquisition to process done.
//   // 根据msg中的信息，计算控制数据ctr
//   /*
//   if msg satisfies some condition...
//   then set ctr to some value...  
//   */

//   // if(msg->tracking){
//   //   if (msg->v_yaw > 1.0){
//   //     packet1.dyaw = 0;
//   //     packet1.dpitch = msg->velocity.z * dt;
//   //     if (msg->yaw>2)
//   //     Isshoot = 1;
//   //   }
//   //   else{
//   //     float r0=msg->radius_1;//??
//   //     yaw_new = msg->yaw +  msg->v_yaw * dt;
//   //     packet1.dyaw = atan((msg->position.y + msg->velocity.y*dt-r0*sin(yaw_new))/(msg->position.x + msg->velocity.x*dt-r0*cos(yaw_new)))- atan((msg->position.y - r0*sin(msg->yaw))/(msg->position.x - r0*cos(msg->yaw)));
//   //     packet1.dpitch = msg->velocity.z * dt;
      
//   //     Isshoot = 1;
//   //   }
//   // }

//   // ctr.down_gimbal_shoot=1;

//   using namespace std::chrono;
//   static time_point<high_resolution_clock> lasttime = std::chrono::system_clock::now();
  

// /* 获取瞄准方案和瞄准位置 */
//   if(msg->tracking) {

//     // shoot the current target
//     // const float delay_time = 0.02;
//     // const float critical_omega = 6.28; // 一秒转两圈的，认为是转速过快，进行反小陀螺
//     tf2::Vector3 aiming_to;
//     // 一段时间后的机器人中心位置和旋转量
//     float r1 = msg->radius_1;
//     // calculate distance 当前时刻装甲板距离
//     float curr_yaw = msg->yaw;
//     float curr_armor_x = msg->position.x - r1 * cos(curr_yaw);
//     float curr_armor_y = msg->position.y - r1 * sin(curr_yaw);;
//     float curr_armor_z = msg->position.z;
//     float dis = sqrt(pow(curr_armor_x, 2) + pow(curr_armor_y, 2) + pow(curr_armor_z, 2));
    
//     float delay_time = dis / shoot_speed_ + shoot_action_delay_ + pipeline_process_latency_ms / 1000.0;
//     // float delay_time = dis / shoot_speed_ + shoot_action_delay_ ;

//     float x_center = msg->position.x + msg->velocity.x * delay_time;
//     float y_center = msg->position.y + msg->velocity.y * delay_time;
//     float yaw_armor = msg->yaw + msg->v_yaw * delay_time;
//     float z_armor = msg->position.z  + msg->velocity.z * delay_time;
//     // 计算装甲板位置
//     float x_armor = x_center - r1 * cos(yaw_armor);
//     float y_armor = y_center - r1 * sin(yaw_armor);

//     bool anti_spinner = abs(msg->v_yaw) > critical_omega_;
//     if(anti_spinner) {
//       // 反小陀螺，瞄准机器人一段时间后所处的位置时的机器人中心
//       // 瞄准机器人中心、正在追踪的装甲板的高度
//       aiming_to.setX(x_center);
//       aiming_to.setY(y_center);
//       aiming_to.setZ(z_armor);
//     } else {
//       // 正常情况下，瞄准机器人一段时间后的装甲板位置
      
//       // float r2 = msg->radius_2;

//       // point x of the gimbal link to

//       // 从原点指向瞄准点的单位向量，用tf计算
//       // 设置aiming_to的值
//       aiming_to.setX(x_armor);
//       aiming_to.setY(y_armor);
//       aiming_to.setZ(z_armor);
//     }

//     // 单位向量
//     aiming_to.normalize();
//     // z轴单位向量
//     tf2::Vector3 odom_z_axis(0, 0, 1);
//     // 做叉乘，得到希望的瞄准方向对应的坐标系的y轴
//     tf2::Vector3 y_axis = odom_z_axis.cross(aiming_to);
//     // 再做一次叉乘，得到z方向
//     tf2::Vector3 z_axis = aiming_to.cross(y_axis);
//     // 得到旋转矩阵
//     tf2::Matrix3x3 aim_matrix;
//     aim_matrix.setValue(aiming_to.x(), y_axis.x(), z_axis.x(),
//                         aiming_to.y(), y_axis.y(), z_axis.y(),
//                         aiming_to.z(), y_axis.z(), z_axis.z());
//     // 计算旋转矩阵的欧拉角
//     double roll, pitch, yaw;
//     aim_matrix.getRPY(roll, pitch, yaw);
//     // it is the same as our robot's zyx euler angle, it is xyz fixed angle
//     // get the transform of gimbal_link frame and its r p y
//     geometry_msgs::msg::TransformStamped transformStamped;
    
//     try{
//       transformStamped = tf_buffer_->lookupTransform("odom", "gimbal_link", tf2::TimePointZero/*ros::Time(0)*/);
//     }
//     catch (tf2::TransformException &ex) {
//       RCLCPP_WARN(this->get_logger(),"%s",ex.what());
//     }
//     // RCLCPP_INFO(this->get_logger(), "transform gotten: q: %f %f %f %f", transformStamped.transform.rotation.x, transformStamped.transform.rotation.y, transformStamped.transform.rotation.z, transformStamped.transform.rotation.w);

//     double gim_roll, gim_pitch, gim_yaw;
//     tf2::Quaternion q(
//       transformStamped.transform.rotation.x,
//       transformStamped.transform.rotation.y,
//       transformStamped.transform.rotation.z,
//       transformStamped.transform.rotation.w);
//     tf2::Matrix3x3 m(q);
//     m.getRPY(gim_roll, gim_pitch, gim_yaw);
//     // log
//     // RCLCPP_INFO(this->get_logger(), "gimbal_link rpy: %f %f %f, aim rpy: %f %f %f", gim_roll, gim_pitch, gim_yaw, roll, pitch, yaw);

//     // Use the roll, pitch, and yaw angles here
//     float space_bullet_delta_pitch = pitch - gim_pitch;
//     ctr.down_gimbal_delta_yaw = yaw - gim_yaw;

//     // watch that the pitch for algorithm gimbal link is opposite to electric control C-board pitch
//     space_bullet_delta_pitch = -space_bullet_delta_pitch;

//     /* pitch 抬升为正方向，和电控统一 */
//     /* 此时已经让云台link 指向了目标 */
//     /* 接下来是补偿枪口下坠 */

    
//     // log
//     #ifdef CALIBERATING_GUN
//     // “太空”子弹的发射角度，一个直线运动、不下坠的子弹
//     RCLCPP_INFO(this->get_logger(), "[no_calib][space bullet] delta pitch: %f, delta yaw: %f, dis: %f", space_bullet_delta_pitch, ctr.down_gimbal_delta_yaw, dis);
//     #endif

//     // 我们认为下坠补偿来源于两个方面，一个是子弹下坠，一个是电机PID控制的静态误差
//     // 当补偿了子弹下坠后的delta pitch和delta yaw小于一定数值时，即可发射
//     // 在补偿了子弹下坠后，进一步补偿电机静态误差，再将控制量发给电机
//     // pitch compensate: bullet dropping

//     float pitch_bullet_compensate = earth_comp_k_ * dis + earth_comp_b_;
//     // 拟合曲线得到，在步兵上，27.2 m/s的射速

//     float earth_bullet_delta_pitch = space_bullet_delta_pitch - pitch_bullet_compensate;

//     #ifdef CALIBERATING_GUN
//     // “地球”子弹的发射角度。实际上，下坠的补偿，也包含了一些机械结构上的静态误差，例如相机位置有小幅度偏差
//     RCLCPP_INFO(this->get_logger(), "[after bullet droppint comp][earth bullet] delta pitch: %f, delta yaw: %f, dis: %f", earth_bullet_delta_pitch, ctr.down_gimbal_delta_yaw, dis);
//     #endif

//     // 发射判据：反小陀螺模式下的判据和正常模式下的判据，应当是三维空间中的距离信息
//     // 计算判据所使用的角度，是在bullet dropping修正后的角度。
//     // 唯一的判断标准：“未来装甲板”是否已经在射击范围内
//     // 是否已经达到了地球子弹的发射角度
//     // 用这两个delta乘以距离（角度比较小，sin theta, tan theta和theta都是同样的了 ）

//     // 装甲板的半高，单位m
//     if(abs(ctr.down_gimbal_delta_pitch * dis) < 0.067 && abs(ctr.down_gimbal_delta_yaw * dis) < 0.067 /*对于大装甲板，应当是0.112，这里暂时统一写成小装甲板的长度，后续有时间再改*/)
//       ctr.down_gimbal_shoot = 1;
//     else
//       ctr.down_gimbal_shoot = 0;

//     // 其实还应当考虑太远的目标，直接不打了，可能中不了
//     // if(!(dis < 7.0))
//     //   ctr.down_gimbal_shoot = 0;
    

//     // 静态误差系数，通过观察pitch输出测量
//     // float static_error_electric_mechanics = 0.03;

//     ctr.down_gimbal_delta_pitch = earth_bullet_delta_pitch + static_error_electric_mechanics_pitch_;
//     ctr.down_gimbal_delta_yaw -= static_error_electric_mechanics_yaw_;
//     #ifdef CALIBERATING_GUN
//     // 为了补偿电控-机械结构静态误差而在发送时提供的增量
//     RCLCPP_INFO(this->get_logger(), "[after static error comp][real send] delta pitch: %f, delta yaw: %f, dis: %f", ctr.down_gimbal_delta_pitch, ctr.down_gimbal_delta_yaw, dis);
//     #endif

//     time_point<high_resolution_clock> nowtime = std::chrono::system_clock::now();
//     int64_t micro_seconds_passed = duration_cast<std::chrono::microseconds>(nowtime -lasttime).count();
//     int64_t milli_seconds_passed = micro_seconds_passed / 1000;
//     if(milli_seconds_passed > shoot_gap_) {
//       ctr.down_gimbal_shoot = 1;
//       lasttime = nowtime;
//     } else {
//       ctr.down_gimbal_shoot = 0;
//     }

//     // 向无下位机控制节点发布控制信息
//     auto_aim_interfaces::msg::DangleShoot msg;
//     msg.delta_pitch = ctr.down_gimbal_delta_pitch;
//     msg.delta_yaw = ctr.down_gimbal_delta_yaw;
//     msg.shoot = ctr.down_gimbal_shoot;
//     float_ctr_publisher_->publish(msg);
//   } else {
//     // not tracking any target, send 0
//     ctr.down_gimbal_shoot = 0;
//     ctr.down_gimbal_delta_pitch = static_error_electric_mechanics_pitch_; // 实际上这会导致云台缓慢下降，更好的方式是：让云台瞄准前方某一个可能出现装甲板或者更容易看到装甲板的位置
//     ctr.down_gimbal_delta_yaw = 0;
//   }

// }


int RMSerialDriver::generate_checksum(const unsigned char *data, int data_len)
{
  int i, checksum;

  checksum = 0;
  for (i = 0; i < data_len; i++) {
    checksum += data[i];
  }

  return checksum & 0xff;
}


int RMSerialDriver::command_settings(int tty_fd, CANUSB_SPEED speed, CANUSB_MODE mode, CANUSB_FRAME frame)
{
  int cmd_frame_len;
  unsigned char cmd_frame[20];

  cmd_frame_len = 0;
  cmd_frame[cmd_frame_len++] = 0xaa;
  cmd_frame[cmd_frame_len++] = 0x55;
  cmd_frame[cmd_frame_len++] = 0x12;
  cmd_frame[cmd_frame_len++] = speed;
  cmd_frame[cmd_frame_len++] = frame;
  cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
  cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
  cmd_frame[cmd_frame_len++] = mode;
  cmd_frame[cmd_frame_len++] = 0x01;
  cmd_frame[cmd_frame_len++] = 0;
  cmd_frame[cmd_frame_len++] = 0;
  cmd_frame[cmd_frame_len++] = 0;
  cmd_frame[cmd_frame_len++] = 0;
  cmd_frame[cmd_frame_len++] = generate_checksum(&cmd_frame[2], 17);

  if (frame_send(tty_fd, cmd_frame, cmd_frame_len) < 0) {
    return -1;
  }

  return 0;
}


int RMSerialDriver::send_data_frame(int tty_fd, CANUSB_FRAME frame, unsigned char id_lsb, unsigned char id_msb, unsigned char data[], int data_length_code)
{
#define MAX_FRAME_SIZE 13
  int data_frame_len = 0;
  unsigned char data_frame[MAX_FRAME_SIZE] = {0x00};

  if (data_length_code < 0 || data_length_code > 8)
  {
    fprintf(stderr, "Data length code (DLC) must be between 0 and 8!\n");
    return -1;
  }

  /* Byte 0: Packet Start */
  data_frame[data_frame_len++] = 0xaa;

  /* Byte 1: CAN Bus Data Frame Information */
  data_frame[data_frame_len] = 0x00;
  data_frame[data_frame_len] |= 0xC0; /* Bit 7 Always 1, Bit 6 Always 1 */
  if (frame == CANUSB_FRAME_STANDARD)
    data_frame[data_frame_len] &= 0xDF; /* STD frame */
  else /* CANUSB_FRAME_EXTENDED */
    data_frame[data_frame_len] |= 0x20; /* EXT frame */
  data_frame[data_frame_len] &= 0xEF; /* 0=Data */
  data_frame[data_frame_len] |= data_length_code; /* DLC=data_len */
  data_frame_len++;

  /* Byte 2 to 3: ID */
  data_frame[data_frame_len++] = id_lsb; /* lsb */
  data_frame[data_frame_len++] = id_msb; /* msb */

  /* Byte 4 to (4+data_len): Data */
  for (int i = 0; i < data_length_code; i++)
    data_frame[data_frame_len++] = data[i];

  /* Last byte: End of frame */
  data_frame[data_frame_len++] = 0x55;

  if (frame_send(tty_fd, data_frame, data_frame_len) < 0)
  {
    fprintf(stderr, "Unable to send frame!\n");
    return -1;
  }

  return 0;
}

int RMSerialDriver::adapter_init(const char *tty_device, int baudrate)
{
  int tty_fd, result;
  struct termios2 tio;

  tty_fd = open(tty_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
  if (tty_fd == -1) {
    fprintf(stderr, "open(%s) failed: %s\n", tty_device, strerror(errno));
    throw std::runtime_error("open() failed");
  }

  result = ioctl(tty_fd, TCGETS2, &tio);
  if (result == -1) {
    fprintf(stderr, "ioctl() failed: %s\n", strerror(errno));
    close(tty_fd);
    throw std::runtime_error("ioctl() failed");
  }

  tio.c_cflag &= ~CBAUD;
  tio.c_cflag = BOTHER | CS8 | CSTOPB;
  tio.c_iflag = IGNPAR;
  tio.c_oflag = 0;
  tio.c_lflag = 0;
  tio.c_ispeed = baudrate;
  tio.c_ospeed = baudrate;

  result = ioctl(tty_fd, TCSETS2, &tio);
  if (result == -1) {
    fprintf(stderr, "ioctl() failed: %s\n", strerror(errno));
    close(tty_fd);
    throw std::runtime_error("ioctl() failed");
  }

  return tty_fd;
}

void RMSerialDriver::ug_ctr_call_back(const auto_aim_interfaces::msg::CmdUpYawVoltShoot::SharedPtr msg)
{
    // Handle the message here
    // For example, print the yaw_volt, pitch_volt and shoot values
    // RCLCPP_INFO(this->get_logger(), "Received message: yaw_volt = %d, pitch_volt = %d, shoot = %d", 
    //     msg->yaw_volt, msg->pitch_volt, msg->shoot);
  
    
}

void RMSerialDriver::receiveData()
{
  
  int /*i,*/ frame_len;
  unsigned char frame[32];
  unsigned char * data_pack; // 8 bytes' data field

  sleep(1);
  
  while (rclcpp::ok()) {
    try {

      frame_len = frame_recv(usb_can_tty_fd, frame, sizeof(frame));

      if (frame_len == -1) {
        printf("Frame recieve error!\n");
        throw std::runtime_error("recieve data error");

      } else {

        if ((frame_len >= 6) &&
            (frame[0] == 0xaa) &&
            ((frame[1] >> 4) == 0xc)) {
          // printf("Frame ID: %02x%02x, Data: ", frame[3], frame[2]);
          uint16_t thisframeid = (frame[3] << 8) | frame[2];
          // 从[4]开始，到倒数第二个字节（包含），是顺序的数据
          data_pack = &frame[4];
          // 对id分类处理，发布不同的消息
          switch (thisframeid)
          {
            case CAN_YAW_UP: {
              // 只关注上云台yaw轴的ecd
              uint16_t yaw_up_ecd = (data_pack[0] << 8) | data_pack[1];
              auto message = std_msgs::msg::UInt16();
              // 在这里填充你的数据
              message.data = yaw_up_ecd;
              ecd_yaw_publisher_->publish(message);
              break;

            }
            default:
              break;
          }

          // for (i = frame_len - 2; i > 3; i--) {
          //   printf("%02x ", frame[i]);
          // }
          // printf("\n");

        } else {
          // printf("Unknown: ");
          // for (i = 0; i <= frame_len; i++) {
            // printf("%02x ", frame[i]);
        }
        // printf("\n");
      }
    } catch (const std::exception & ex) {
      RCLCPP_ERROR_THROTTLE(
        get_logger(), *get_clock(), 20, "Error while receiving data: %s", ex.what());
      reopenPort();
    }
  }
}

void RMSerialDriver::sendOneCtrData(ctr_data_raw_float & myctr) {

  ctr_data cmd;
  
  encode_ctr_data(&myctr, &cmd);

  unsigned char binary_id_lsb = 0, binary_id_msb = 0;
  int id = 5;
  int error = 0;
  binary_id_lsb = id & 0xff;
  binary_id_msb = (id >> 8) & 0xff;
  try {
    {
      std::lock_guard<std::mutex> lock(usb_can_write_mutex);
      error = send_data_frame(usb_can_tty_fd, CANUSB_FRAME_STANDARD, binary_id_lsb, binary_id_msb, (unsigned char *)&cmd, sizeof(cmd));
    }
    if(error == -1)
      throw std::runtime_error("send_data_frame error");
      
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while sending data: %s", ex.what());
    reopenPort();
  }
}

void RMSerialDriver::timerCallback()
{
    // 检查是否超过1秒
    auto current_time = std::chrono::steady_clock::now();

    // 计算距离上次收到自瞄信息的时间
    auto time_since_last_aim_info = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_aim_info_time_).count();
    auto delta_s = time_since_last_aim_info / 1000.0;
    if (delta_s > 1.0)
    {
        std::cout<<delta_s << std::endl;
        // RCLCPP_INFO(this->get_logger(),"delta_s: %f and rotate", delta_s);
        // 向串口发送瞄准信息
        float period = 1.0; // seconds
        float ampli = 0.1;
        // 前一半上升，下一半下降
        float phase = std::fmod(delta_s, period) / period;
        float pitch_offset = (phase < (period / 2)) ? ampli: -ampli;

        ctr_data_raw_float myctr;
        myctr.down_gimbal_delta_pitch = pitch_offset;

        myctr.down_gimbal_delta_yaw = 0.1;
        myctr.down_gimbal_shoot = 0;
        myctr.chassis_yaw = chassis_yaw_spin_;
        sendOneCtrData(myctr);
    }
}

void RMSerialDriver::sendData(const auto_aim_interfaces::msg::DangleShoot::SharedPtr msg)
{
  // info that we send
  // last_aim_time_ = this->now();
  last_aim_info_time_ = std::chrono::steady_clock::now();
  
  const static std::map<std::string, uint8_t> id_unit8_map{
    {"", 0},  {"outpost", 0}, {"1", 1}, {"1", 1},     {"2", 2},
    {"3", 3}, {"4", 4},       {"5", 5}, {"guard", 6}, {"base", 7}};
  
  ctr_data_raw_float myctr;
  myctr.down_gimbal_delta_pitch = msg->delta_pitch;
  myctr.down_gimbal_delta_yaw = msg->delta_yaw;
  myctr.down_gimbal_shoot = msg->shoot ? 1 : 0;
  
  sendOneCtrData(myctr);
}

void RMSerialDriver::getParams()
{
  try {
    earth_comp_k_ = declare_parameter<float>("earth_comp_k", 0.0);
    earth_comp_b_ = declare_parameter<float>("earth_comp_b", 0.0);
    static_error_electric_mechanics_pitch_ = declare_parameter<float>("static_error_electric_mechanics_pitch", 0.0);
    static_error_electric_mechanics_yaw_ = declare_parameter<float>("static_error_electric_mechanics_yaw", 0.0);
    shoot_action_delay_ = declare_parameter<float>("shoot_action_delay", 0.0);
    shoot_speed_ = declare_parameter<float>("shoot_speed", 0.0);
    critical_omega_ = declare_parameter<float>("critical_omega", 0.0);
    shoot_gap_ = declare_parameter<float>("shoot_gap", 0.0);

    RCLCPP_INFO(get_logger(), "gun caliberation coef:");
    RCLCPP_INFO(get_logger(), "earth_comp_k: %f, earth_comp_b:%f, static_error_electric_mechanics: %f shoot_action_delay_: %f, shoot_speed_: %f, critical_omega: %f", 
      earth_comp_k_, earth_comp_b_, static_error_electric_mechanics_pitch_,
      shoot_action_delay_, shoot_speed_, critical_omega_);
    
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "gun caliberation coef not set properly");
    throw ex;
  }
  try {
    device_name_ = declare_parameter<std::string>("device_name", "");
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The device name provided was invalid");
    throw ex;
  }


  // try {
  //   baud_rate = declare_parameter<int>("baud_rate", 0);
  // } catch (rclcpp::ParameterTypeException & ex) {
  //   RCLCPP_ERROR(get_logger(), "The baud_rate provided was invalid");
  //   throw ex;
  // }

  // try {
  //   const auto fc_string = declare_parameter<std::string>("flow_control", "");

  //   if (fc_string == "none") {
  //     fc = FlowControl::NONE;
  //   } else if (fc_string == "hardware") {
  //     fc = FlowControl::HARDWARE;
  //   } else if (fc_string == "software") {
  //     fc = FlowControl::SOFTWARE;
  //   } else {
  //     throw std::invalid_argument{
  //       "The flow_control parameter must be one of: none, software, or hardware."};
  //   }
  // } catch (rclcpp::ParameterTypeException & ex) {
  //   RCLCPP_ERROR(get_logger(), "The flow_control provided was invalid");
  //   throw ex;
  // }

  // try {
  //   const auto pt_string = declare_parameter<std::string>("parity", "");

  //   if (pt_string == "none") {
  //     pt = Parity::NONE;
  //   } else if (pt_string == "odd") {
  //     pt = Parity::ODD;
  //   } else if (pt_string == "even") {
  //     pt = Parity::EVEN;
  //   } else {
  //     throw std::invalid_argument{"The parity parameter must be one of: none, odd, or even."};
  //   }
  // } catch (rclcpp::ParameterTypeException & ex) {
  //   RCLCPP_ERROR(get_logger(), "The parity provided was invalid");
  //   throw ex;
  // }

  // try {
  //   const auto sb_string = declare_parameter<std::string>("stop_bits", "");

  //   if (sb_string == "1" || sb_string == "1.0") {
  //     sb = StopBits::ONE;
  //   } else if (sb_string == "1.5") {
  //     sb = StopBits::ONE_POINT_FIVE;
  //   } else if (sb_string == "2" || sb_string == "2.0") {
  //     sb = StopBits::TWO;
  //   } else {
  //     throw std::invalid_argument{"The stop_bits parameter must be one of: 1, 1.5, or 2."};
  //   }
  // } catch (rclcpp::ParameterTypeException & ex) {
  //   RCLCPP_ERROR(get_logger(), "The stop_bits provided was invalid");
  //   throw ex;
  // }

  // device_config_ =
  //   std::make_unique<drivers::serial_driver::SerialPortConfig>(baud_rate, fc, pt, sb);
}

int RMSerialDriver::frame_recv(int tty_fd, unsigned char *frame, int frame_len_max)
{
  int result, frame_len, checksum;
  unsigned char byte;

  // if (print_traffic)
  //   fprintf(stderr, "receiving <<< ");

  frame_len = 0;
  bool program_running = true;
  while (program_running) {
    result = read(tty_fd, &byte, 1);
    if (result == -1) {
      if (errno != EAGAIN && errno != EWOULDBLOCK) {
        fprintf(stderr, "read() failed: %s\n", strerror(errno));
        return -1;
      }

    } else if (result > 0) {
      // if (print_traffic)
      //   fprintf(stderr, "%02x ", byte);

      if (frame_len == frame_len_max) {
        fprintf(stderr, "frame_recv() failed: Overflow\n");
        return -1;
      }

      frame[frame_len++] = byte;

      if (frame_is_complete(frame, frame_len)) {
        break;
      }
    }

    usleep(10);
  }

  // if (print_traffic)
  //   fprintf(stderr, "\n");

  /* Compare checksum for command frames only. */
  if ((frame_len == 20) && (frame[0] == 0xaa) && (frame[1] == 0x55)) {
    checksum = generate_checksum(&frame[2], 17);
    if (checksum != frame[frame_len - 1]) {
      fprintf(stderr, "frame_recv() failed: Checksum incorrect\n");
      return -1;
    }
  }

  return frame_len;
}

void RMSerialDriver::reopenPort()
{
  RCLCPP_WARN(get_logger(), "Attempting to reopen port");
  try {
    // if (serial_driver_->port()->is_open()) {
    //   serial_driver_->port()->close();
    // }
    // serial_driver_->port()->open();
    close(usb_can_tty_fd);
    usb_can_tty_fd = adapter_init(device_name_.c_str(), CANUSB_TTY_BAUD_RATE_DEFAULT);
    RCLCPP_INFO(get_logger(), "Successfully reopened port");
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while reopening port: %s", ex.what());
    if (rclcpp::ok()) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      reopenPort();
    }
  }
}


void RMSerialDriver::setParam(const rclcpp::Parameter & param)
{
  if (!detector_param_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping parameter set");
    return;
  }

  if (
    !set_param_future_.valid() ||
    set_param_future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
    RCLCPP_INFO(get_logger(), "Setting detect_color to %ld...", param.as_int());
    set_param_future_ = detector_param_client_->set_parameters(
      {param}, [this, param](const ResultFuturePtr & results) {
        for (const auto & result : results.get()) {
          if (!result.successful) {
            RCLCPP_ERROR(get_logger(), "Failed to set parameter: %s", result.reason.c_str());
            return;
          }
        }
        RCLCPP_INFO(get_logger(), "Successfully set detect_color to %ld!", param.as_int());
        initial_set_param_ = true;
      });
  }
}

void RMSerialDriver::resetTracker()
{
  if (!reset_tracker_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping tracker reset");
    return;
  }

  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
  reset_tracker_client_->async_send_request(request);
  RCLCPP_INFO(get_logger(), "Reset tracker!");
}


int RMSerialDriver::frame_send(int tty_fd, const unsigned char *frame, int frame_len)
{
  int result ;//, i;
  // bool print_traffic = true;
  // if (print_traffic) {
  //   printf("sending: >>> ");
  //   for (i = 0; i < frame_len; i++) {
  //     printf("%02x ", frame[i]);
  //   }
  //   if (print_traffic > 1) {
  //     printf("    '");
  //     for (i = 4; i < frame_len - 1; i++) {
  //       printf("%c", isalnum(frame[i]) ? frame[i] : '.');
  //     }
  //     printf("'");
  //   }
  //   printf("\n");
  // }

  result = write(tty_fd, frame, frame_len);
  if (result == -1) {
    fprintf(stderr, "write() failed: %s\n", strerror(errno));
    return -1;
  }

  return frame_len;
}



// int RMSerialDriver::frame_recv(int tty_fd, unsigned char *frame, int frame_len_max)
// {
//   // bool print_traffic = true;
//   int result, frame_len, checksum;
//   unsigned char byte;

//   // if (print_traffic)
//   //   fprintf(stderr, "receiving <<< ");

//   frame_len = 0;
//   while (program_running) {
//     result = read(tty_fd, &byte, 1);
//     if (result == -1) {
//       if (errno != EAGAIN && errno != EWOULDBLOCK) {
//         fprintf(stderr, "read() failed: %s\n", strerror(errno));
//         return -1;
//       }

//     } else if (result > 0) {
//       // if (print_traffic)
//       //   fprintf(stderr, "%02x ", byte);

//       if (frame_len == frame_len_max) {
//         fprintf(stderr, "frame_recv() failed: Overflow\n");
//         return -1;
//       }

//       frame[frame_len++] = byte;

//       if (frame_is_complete(frame, frame_len)) {
//         break;
//       }
//     }

//     usleep(10);
//   }

//   // if (print_traffic)
//   //   fprintf(stderr, "\n");

//   /* Compare checksum for command frames only. */
//   if ((frame_len == 20) && (frame[0] == 0xaa) && (frame[1] == 0x55)) {
//     checksum = generate_checksum(&frame[2], 17);
//     if (checksum != frame[frame_len - 1]) {
//       fprintf(stderr, "frame_recv() failed: Checksum incorrect\n");
//       return -1;
//     }
//   }

//   return frame_len;
// }



int RMSerialDriver::frame_is_complete(const unsigned char *frame, int frame_len)
{
  if (frame_len > 0) {
    if (frame[0] != 0xaa) {
      /* Need to sync on 0xaa at start of frames, so just skip. */
      return 1;
    }
  }

  if (frame_len < 2) {
    return 0;
  }

  if (frame[1] == 0x55) { /* Command frame... */
    if (frame_len >= 20) { /* ...always 20 bytes. */
      return 1;
    } else {
      return 0;
    }
  } else if ((frame[1] >> 4) == 0xc) { /* Data frame... */
    if (frame_len >= (frame[1] & 0xf) + 5) { /* ...payload and 5 bytes. */
      return 1;
    } else {
      return 0;
    }
  }

  /* Unhandled frame type. */
  return 1;
}

}  // namespace rm_serial_driver

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rm_serial_driver::RMSerialDriver)
